#include "IGITrackingDataGUI.h"

#include <sys/types.h>
#include <sys/stat.h>

#include "itkTextOutput.h"

#include <vtkFileOutputWindow.h>
#include <vtkOutputWindow.h>
#include <vtkSmartPointer.h>

#include <QMessageBox>

#include "igstkTransformObserver.h"
#include "igstkTransformFileWriter.h"
#include "igstkRigidTransformXMLFileWriter.h"

#include "../IGIConfigurationData.h"

#define VIEW_3D_REFRESH_RATE 10
#define DEFAULT_ZOOM_FACTOR 1.5
#define TRACKER_FREQUENCY 20

/** ---------------------------------------------------------------
*     Constructor
* -----------------------------------------------------------------
*/
IGITrackingDataGUI::IGITrackingDataGUI()
{
  /** Initializations */
  m_GUI.setupUi(this);
  this->CreateActions();
  m_GUIQuit = false;
  
  // get current application path
  QString path = QApplication::applicationDirPath();
  path.truncate(path.lastIndexOf("/Programs"));
  m_CurrentPath = path + "/Programs";
  m_ConfigDir = path + "/" + IGIConfigurationData::CONFIGURATION_FOLDER;

  m_PositionIterator = 1;
 
  // suppress itk output window
  itk::OutputWindow::SetInstance(itk::TextOutput::New());

  vtkSmartPointer<vtkFileOutputWindow> fileOutputWindow =
      vtkSmartPointer<vtkFileOutputWindow>::New();
  fileOutputWindow->SetFileName( "output.txt" );

  vtkOutputWindow* outputWindow = vtkOutputWindow::GetInstance();
  if ( outputWindow )
  {
    outputWindow->SetInstance( fileOutputWindow );
  }

  m_WorldReference  = igstk::AxesObject::New();
  m_WorldReference->SetSize(100.0, 100.0, 100.0);

  QTimer *pulseTimer = new QTimer();
  connect(pulseTimer, SIGNAL(timeout()), this, SLOT(PulseTimerEvent()));
  pulseTimer->start(10);

  m_PointAquisitionPulseTimer = new QTimer();
  connect(m_PointAquisitionPulseTimer, SIGNAL(timeout()), this, SLOT(PointAquisition()));
  m_PointAquisitionStarted = false;

  // GUI status
  m_GUI.CheckPositionButton->setEnabled(false);
  m_GUI.StartButton->setEnabled(false);

  m_GUI.LED102->hide();
  m_GUI.LED403->hide();
  m_GUI.LED502->hide();
  m_GUI.LED201->hide();
  m_GUI.LED202->hide();
}

/** -----------------------------------------------------------------
*     Connect GUI elements with actions
*  -----------------------------------------------------------------
*/
void IGITrackingDataGUI::CreateActions()
{
  connect(m_GUI.QuitPushButton, SIGNAL(clicked()), this, SLOT(OnQuitAction()));
  connect(m_GUI.helpButton, SIGNAL(clicked()), this, SLOT(showHelp()));

  connect(m_GUI.CheckPositionButton, SIGNAL(clicked()), this, SLOT(CheckMarkerPosition()));
  connect(m_GUI.StartButton, SIGNAL(clicked()), this, SLOT(TogglePointAquisition()));

  connect(m_GUI.CameraCalibFileButton, SIGNAL( clicked()), this, SLOT(SelectCameraCalibrationFile()));

  connect(m_GUI.InitializeButton, SIGNAL( clicked()), this, SLOT(StartInitialization()));
}

void IGITrackingDataGUI::SelectCameraCalibrationFile()
{
  QString fName = QFileDialog::getOpenFileName(this,
      "Select Camera Calibration File.",
      m_ConfigDir,
      "Camera Calibration File (*.yml)");

  if(fName.isNull())
    return;
  if (fName.isEmpty())
  {
    handleMessage("No camera calibration file [*.yml] was selected.\n",1);
    return;
  }

  m_GUI.CameraCalibFileEdit->setText(fName);
  m_CameraParametersFile = fName.toStdString(); 
}

bool IGITrackingDataGUI::InitializeTrackerAction()
{
  igstk::Transform identity;
  identity.SetToIdentity(igstk::TimeStamp::GetLongestPossibleTime());

  this->m_ErrorObserver = NULL;
  //create error observer
  this->m_ErrorObserver = ErrorObserver::New();
  //create tracker
  m_ArucoTracker = igstk::ArucoTracker::New(); 
  m_ArucoTracker->RequestSetFrequency( TRACKER_FREQUENCY );
  m_ArucoTracker->RequestSetTransformAndParent(identity, m_WorldReference);

  // Set marker size in mm
  this->m_ArucoTracker->SetMarkerSize(50);

  if(m_CameraParametersFile.empty())
  {
    handleMessage( "Camera calibration file not defined.\n" , 1 );
    return false;
  }
  
  // load camera calibration file
  m_ArucoTracker->SetCameraParametersFromYAMLFile(m_CameraParametersFile);
 
  //observe all possible errors generated by the tracker
  unsigned long observerID = m_ArucoTracker->AddObserver(
                             igstk::IGSTKErrorEvent(), this->m_ErrorObserver );
  m_ArucoTracker->RequestOpen();
  m_ArucoTracker->RemoveObserver( observerID );

  if( this->m_ErrorObserver->ErrorOccured() )
  {
    this->m_ErrorObserver->GetErrorMessage( this->m_ErrorMessage );
    this->m_ErrorObserver->ClearError();
    handleMessage( this->m_ErrorMessage, 1 );
    return false;
  }
  else  //attach the tools 
  {
    if(m_GUI.check102->isChecked())
    { 
      m_MarkerIDs.push_back("102");
      m_GUI.LED102->show();
    }
    
    if(m_GUI.check403->isChecked())
    {
      m_MarkerIDs.push_back("403");
      m_GUI.LED403->show();
    }

    if(m_GUI.check502->isChecked())
    {
      m_MarkerIDs.push_back("502");
      m_GUI.LED502->show();
    }
 
    if(m_GUI.check201->isChecked())
    {
      m_MarkerIDs.push_back("201");
      m_GUI.LED201->show();
    }
    
    if(m_GUI.check202->isChecked())
    {
      m_MarkerIDs.push_back("202");
      m_GUI.LED202->show();
    }

    for(vector<std::string>::iterator it = m_MarkerIDs.begin();it!=m_MarkerIDs.end();it++)
    {
      std::string markerID = *it;

      m_ArucoTrackerToolMap[markerID] = igstk::ArucoTrackerTool::New();
      m_ArucoTrackerToolMap[markerID]->RequestSetMarkerName(atoi(markerID.c_str()));
      m_ArucoTrackerToolMap[markerID]->RequestConfigure();
      m_ArucoTrackerToolMap[markerID]->RequestAttachToTracker( m_ArucoTracker );
    }
  }
 
  m_ArucoTracker->AddObserver(igstk::TrackerStartTrackingEvent(),
  this->m_ErrorObserver);

  m_ArucoTracker->RequestStartTracking();

  //check that start was successful
  if( this->m_ErrorObserver->ErrorOccured() )
  {
    this->m_ErrorObserver->GetErrorMessage( this->m_ErrorMessage );
    this->m_ErrorObserver->ClearError();
    handleMessage( "Tracker start error\n" , 1 );
    return false;
  }
 
  m_GUI.CameraCalibFileButton->setDisabled(true);
  m_GUI.check102->setDisabled(true);
  m_GUI.check201->setDisabled(true);
  m_GUI.check202->setDisabled(true);
  m_GUI.check403->setDisabled(true);
  m_GUI.check502->setDisabled(true);

  return true;
}

void IGITrackingDataGUI::TogglePointAquisition()
{
  if(m_PointAquisitionStarted)
  {
    m_PointAquisitionStarted = false;
    m_PointAquisitionPulseTimer->stop();
    m_GUI.StartButton->setText("Start");
    m_PositionIterator++;
    for(vector<std::string>::iterator it = m_MarkerIDs.begin();it!=m_MarkerIDs.end();it++)
    {
      std::string markerID = *it;
      m_Logfiles[markerID]->close();
    }
  }
  else
  {
    m_Logfiles.clear();
    for(vector<std::string>::iterator it = m_MarkerIDs.begin();it!=m_MarkerIDs.end();it++)
    {
      std::string markerID = *it;
      std::string fileName = m_ConfigDir.toStdString() + "/LogFile_ID" + markerID + "_" + QString::number(m_PositionIterator).toStdString() + ".txt";
      m_Logfiles[markerID] = new std::ofstream(fileName.c_str()); 
    }

    unsigned int delay = 5000;
    std::ostringstream msg;
    for(unsigned int i=delay; i>0; i-=1000 )
    {
      delay-=1000;
      msg << "Data acquisition starts in "<<(int)(i/1000)<<" seconds.";
      m_GUI.StartButton->setText(QString(msg.str().c_str()));
      QApplication::processEvents();
      igstk::PulseGenerator::Sleep(1000);
      msg.str("");
    }
    m_PointAquisitionStarted = true;
    m_GUI.StartButton->setText("Stop");
    m_PointAquisitionPulseTimer->start(TRACKER_FREQUENCY);

    QApplication::processEvents();
  }
}

void IGITrackingDataGUI::PointAquisition()
{
  for(vector<std::string>::iterator it = m_MarkerIDs.begin();it!=m_MarkerIDs.end();it++)
  {
    std::string markerID = *it;

    // get transform
    typedef igstk::TransformObserver ObserverType;
    ObserverType::Pointer transformObserver = ObserverType::New();
    transformObserver->ObserveTransformEventsFrom( m_ArucoTrackerToolMap[markerID] );
    transformObserver->Clear();
  
    m_ArucoTrackerToolMap[markerID]->RequestComputeTransformTo( m_WorldReference );

    if( !transformObserver->GotTransform() )
    {  
      //if error occurs we silently continue
      return;
    }
  
    igstk::Transform transform;
    transform = transformObserver->GetTransform();

    if(transform.IsValidNow())
    {
      igstk::Transform::VectorType position = transform.GetTranslation();
      igstk::Transform::VersorType v = transform.GetRotation();

      *m_Logfiles[markerID] << QString::number(transform.GetStartTime(),'g',18).toStdString() << " " << position[0] << " " << position[1] << " " << position[2] << " ";
      *m_Logfiles[markerID] << v.GetX() << " " << v.GetY() << " "<< v.GetZ() << " "<< v.GetW() << std::endl;
      SetMarkerLED(markerID,true);
    }
    else
    {
      SetMarkerLED(markerID,false);
    }
  }
}

void IGITrackingDataGUI::SetMarkerLED( std::string markerID, bool state)
{
  if(strcmp(markerID.c_str(), "102") == 0)
  {
    if(state)
      m_GUI.LED102->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/greenLED.png")));
    else
      m_GUI.LED102->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/redLED.png")));
  }
  else if(strcmp(markerID.c_str(), "403") == 0)
  {
    if(state)
      m_GUI.LED403->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/greenLED.png")));
    else
      m_GUI.LED403->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/redLED.png")));
  }
  else if(strcmp(markerID.c_str(), "502") == 0)
  {
    if(state)
      m_GUI.LED502->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/greenLED.png")));
    else
      m_GUI.LED502->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/redLED.png")));
  }
  else if(strcmp(markerID.c_str(), "201") == 0)
  {
    if(state)  
      m_GUI.LED201->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/greenLED.png")));
    else
      m_GUI.LED201->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/redLED.png")));
  }
  else if(strcmp(markerID.c_str(), "202") == 0)
  {
    if(state)
      m_GUI.LED202->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/greenLED.png")));
    else
      m_GUI.LED202->setPixmap(QPixmap(QString::fromUtf8(":/Images/Images/redLED.png")));
  }
}

void IGITrackingDataGUI::keyPressEvent ( QKeyEvent * event)
{
  switch ( event->key() )
  {
    case Qt::Key_C: CheckMarkerPosition();
        break;
    case Qt::Key_S: TogglePointAquisition(); 
        break;
    default:
        return;
  }
}

void IGITrackingDataGUI::StartInitialization()
{
  if(m_CameraParametersFile.empty())
  {
    QMessageBox::information(this, "IGI TrackingData", "Camera calibration file not defined.");
    return;
  }

  if(!InitializeTrackerAction()) 
    return;

  // GUI status
  m_GUI.CheckPositionButton->setEnabled(true);
  m_GUI.StartButton->setEnabled(true);
  m_GUI.InitializeButton->setEnabled(false);
}

void IGITrackingDataGUI::PulseTimerEvent()
{
  igstk::PulseGenerator::CheckTimeouts();
}

void IGITrackingDataGUI::showHelp()
{
  QDialog* helpBox = new QDialog(this);
  helpBox->setWindowTitle("Help");

  QTextBrowser* browser = new QTextBrowser(helpBox); 
  browser->setSource(QUrl::fromLocalFile(m_CurrentPath + "/READMETrackingData.html"));
  browser->setWindowTitle("Help");

  QPushButton* okButton = new QPushButton(helpBox);
  connect(okButton, SIGNAL(clicked()), helpBox, SLOT(close()));
  okButton->setGeometry(QRect(150, 260, 100, 25));
  okButton->setText("Quit");

  QVBoxLayout* helpLayout = new QVBoxLayout(helpBox);
  helpLayout->addWidget(browser);
  helpLayout->addWidget(okButton);

  helpBox->resize(500,500);
  helpBox->show();
}

/**-----------------------------------------------------------------
*  Show quit dialog
*---------------------------------------------------------------------
*/
void IGITrackingDataGUI::OnQuitAction()
{
  QMessageBox::StandardButton value = 
    QMessageBox::information(this,
    "IGITrackingData:", "Are you sure you want to quit?",
    QMessageBox::Yes | QMessageBox::No );

  if( value == QMessageBox::Yes )
  {
    this->close();
    m_GUIQuit = true;
  }
}

bool IGITrackingDataGUI::HasQuit() 
{
  return m_GUIQuit;
}

/** -----------------------------------------------------------------
*     Destructor
*  -----------------------------------------------------------------
*/
IGITrackingDataGUI::~IGITrackingDataGUI()
{
}

void IGITrackingDataGUI::CheckMarkerPosition()
{
  cv::Mat currentImage = m_ArucoTracker->GetCurrentVideoFrame(); 
  cv::imshow("video",currentImage);
  cv::waitKey(2000);
  cv::destroyWindow("video");
}

/** -----------------------------------------------------------------
*  Construct an error observer for all the possible errors that occur in 
*   the observed IGSTK components.
*---------------------------------------------------------------------
*/
IGITrackingDataGUI::ErrorObserver::ErrorObserver() : m_ErrorOccured( false )
{ //serial communication errors
  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>( igstk::OpenPortErrorEvent().GetEventName(),
                                       "Error opening com port." ) );
  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>( igstk::ClosePortErrorEvent().GetEventName(),
                                        "Error closing com port." ) );
  //tracker errors
  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>( igstk::TrackerOpenErrorEvent().GetEventName(),
      "Error opening tracker communication." ) );
  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>( 
      igstk::TrackerInitializeErrorEvent().GetEventName(),
      "Error initializing tracker." ) );
  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>( 
      igstk::TrackerStartTrackingErrorEvent().GetEventName(),
      "Error starting tracking." ) );
  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>( 
      igstk::TrackerStopTrackingErrorEvent().GetEventName(),
      "Error stopping tracking." ) );
  this->m_ErrorEvent2ErrorMessage.insert(
    std::pair<std::string,std::string>( 
      igstk::TrackerCloseErrorEvent().GetEventName(),
      "Error closing tracker communication." ) );
}

/** -----------------------------------------------------------------
*   When an error occurs in an IGSTK component it will invoke this method 
*   with the appropriate error event object as a parameter.
*---------------------------------------------------------------------
*/
void IGITrackingDataGUI::ErrorObserver::Execute( const itk::Object * itkNotUsed(caller), 
  const itk::EventObject & event ) throw (std::exception)
{
  std::map<std::string,std::string>::iterator it;
  std::string className = event.GetEventName();
  it = this->m_ErrorEvent2ErrorMessage.find(className);

  if( it != this->m_ErrorEvent2ErrorMessage.end() )
  {
    this->m_ErrorOccured = true;
    this->m_ErrorMessage = (*it).second;
  }
  //if the event we got wasn't in the error events map then we
  //silently ignore it
}

/** -----------------------------------------------------------------
*   When an error occurs in an IGSTK component it will invoke this method 
*   with the appropriate error event object as a parameter.
*---------------------------------------------------------------------
*/
void IGITrackingDataGUI::ErrorObserver::Execute( itk::Object *caller, 
  const itk::EventObject & event ) throw (std::exception)
{
  const itk::Object * constCaller = caller;
  this->Execute(constCaller, event);
}

